#include "datmoheaders.h"

using namespace NEWMAT;
using namespace DATMO;
using namespace pcl;

//Class definition
class Detection {
public:
	Detection();
	void Distance_Image_Callback(const sensor_msgs::Image::ConstPtr& msg);
	void PointCloud_Callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
	int iteration;
	tf::TransformListener tf_listener;
	string trial_name;
protected:
	RobotState str_RobotState;
	DistanceImage str_DistImage;
	DistanceImage str_LastDistImage;
	DistanceImage str_LastLastDistImage;
	BackgroundReference str_BackgroundRef;

};

//Class Initialization
Detection::Detection(){
	str_RobotState.b_isMovement = false;
	str_RobotState.s_Initial = 0;
	iteration = 0;
}

//Each time a distance image is received
void Detection::Distance_Image_Callback(const sensor_msgs::Image::ConstPtr& msg)
{
	//Receive distance matrix and convert it to a matrix
	iteration++;
	ROS_INFO("Iteracion %d", iteration);
	float* array = (float*)&(msg->data[0]);
	DistanceImage D;
	D.tf_Distance = Matrix((int)msg->height,(int)msg->width);
	int i = 1, j= 1;
	for(int k=0;k<(int)(msg->width * msg->height);k++)
	{
		D.tf_Distance(i,j) = (double)array[k];
		j++;
		if (j > (int)msg->width)
		{
			i++;j=1;
		}
	}
	//Save as DistanceImage struct
	D.s_height = (int)msg->height;
	D.s_width = (int)msg->width;
	D.CollisionPoints.header = msg->header;
	tf::StampedTransform transform;
	tf_listener.waitForTransform(msg->header.frame_id, "/world",  msg->header.stamp, ros::Duration(2.0));
	tf_listener.lookupTransform("/kinect", "/world", msg->header.stamp, transform);
	D.CollisionPoints = cl_CollisionMatrix::CollisionMatrix(D,transform);
	//Actualize images
	str_LastLastDistImage = str_LastDistImage;
	str_LastDistImage = str_DistImage;
	str_DistImage = D;
	if (str_RobotState.s_Initial == 0)
	{
		str_BackgroundRef.tf_Dref = Matrix(D.s_height,D.s_width);
		str_BackgroundRef.tf_Dref = str_DistImage.tf_Distance;
		str_BackgroundRef.tf_Vref = Matrix(D.s_height,D.s_width);
		str_BackgroundRef.tf_Vref = 100000;
		ROS_INFO("%f probando",str_BackgroundRef.tf_Vref(34,75));
		str_RobotState.s_Initial++;
	}
	if (str_RobotState.s_Initial>3)
	{
		//Two different options: robot still and robot moving
		if(str_RobotState.b_isMovement == false) //Robot is not moving
		{
			if(!cl_CompareLaser::CompareLaser(str_DistImage,str_LastDistImage))
			{
				//Detect Objects

				DetectedObjects str_DetectedObjects = cl_ObjectDetection::ObjectDetection(str_DistImage,str_LastDistImage);
				cl_PrintFunctions::printMatrix(str_DistImage.tf_Distance,"ACTMAT",iteration, trial_name);
				cl_PrintFunctions::printMatrix2(str_DistImage.tf_Distance,"ACTMATSEC",iteration, trial_name);
				cl_PrintFunctions::printMatrix(str_LastDistImage.tf_Distance,"LASTMAT",iteration, trial_name);
				cl_PrintFunctions::printMatrix(str_DetectedObjects.m_Objects,"OBJ",iteration, trial_name);
				//Update Reference
				ROS_INFO("Detection complete");
				str_BackgroundRef = cl_UpdateReference::UpdateReference(str_BackgroundRef,str_DistImage,str_LastDistImage,str_LastLastDistImage,str_DetectedObjects.m_Objects);

				cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Dref,"DREF",iteration, trial_name);
				cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Vref,"aVREF",iteration, trial_name);
				//Objects2Real
				if (str_DetectedObjects.i_NumberOfObjects > 0)
				{
					ROS_INFO("Entra en paso a objetos reales");
					ros::Time actTime = ros::Time::now();
					cl_DynamicObjects2 DynamicObjects_act = cl_Objects2Real::Objects2Real(str_DistImage, str_DetectedObjects, str_BackgroundRef.tf_Dref,
													 actTime, str_RobotState.RobotID);
					cl_PrintFunctions::printDynObjList(DynamicObjects_act,"OBJREAL",iteration, trial_name);
					ROS_INFO("Entra en identificacion");
					str_RobotState.DynObjects = cl_ObjectIdentification::ObjectIdentification(DynamicObjects_act,
												str_RobotState.DynObjects,actTime,str_RobotState.RobotID);
				}
			}
			else
			{
			  Matrix auxMat(str_BackgroundRef.tf_Dref.Nrows(),str_BackgroundRef.tf_Dref.Ncols());
			  str_BackgroundRef = cl_UpdateReference::UpdateReference(str_BackgroundRef,str_DistImage,str_LastDistImage,str_LastLastDistImage,auxMat);
			  cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Dref,"DREF",iteration, trial_name);
			  cl_PrintFunctions::printMatrix(str_BackgroundRef.tf_Vref,"aVREF",iteration, trial_name);
			}
		}
		else //Robot is moving
		{
		//	cl_isMovement::isMovement();
		}
	}
	else
	{
	  str_RobotState.s_Initial++;

	}
}

void Detection::PointCloud_Callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{

}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "datmo2");
	//ROS_INFO("1");
	ros::NodeHandle n;
	//ROS_INFO("2");
	Detection *controller = new Detection();

	//ROS Subscribers
	//ROS_INFO("ej");
	//Create time string for test
	time_t now = time(NULL);
	struct tm* now_time = gmtime(&now);
	std::stringstream time_str;
	time_str << now_time->tm_year+1900 <<"_" << now_time->tm_mon+1 <<"_";
	time_str << now_time->tm_mday <<"_" << now_time->tm_hour <<"_";
	time_str << now_time->tm_min <<"_" << now_time->tm_sec <<"_";
	controller->trial_name = time_str.str();
	ros::Subscriber distance_image_sub = n.subscribe("camera/depth/image", 1000, &Detection::Distance_Image_Callback, controller);
	ros::Subscriber pointcloud_sub = n.subscribe("camera/rgb/points",1000,&Detection::PointCloud_Callback,controller);
	//ROS Publishers
	ros::spin();
	delete controller;

	return 0;
}

